/***************************************************************************
 * @version   --> v1.0
 * @date      -->2021-04-18
 * @brief     -->姿态管理模块
 *            1.通过串口接收上位机发送的指令，主要控制四个飞轮和三轴磁力矩器，
 *              从而调节姿态，同时把当前立方星的姿态信息通过串口发送给上位机
 *            2.通过Arduino串口调试时，可以调节飞轮转速和控制磁力矩器产生的力矩
 *@update     -->
 **************************************************************************/

/*Includes --------------------------------------------------------------*/
#include <Wire.h>       // IIC通信
#include <SCoop.h>      // 使用SCoop线程库
#include <string.h>     // 使用内存拷贝
#include "IMUGY85.h"    // 姿态敏感器
#include <MsTimer2.h>    // 使用定时器

/* Private macro --------------------------------------------------------*/
// 宏开关，可启用Serial用于串口调试
// 正式编译的时候务必注释掉
//#define SERIAL_DEBUG

/* Private define ------------------------------------------------------*/
// 引脚定义
const int WHEEL_DIR0 = 53;   // 1号电机方向控制引脚 53
const int WHEEL_FG0 = 3;    // 1号电机霍尔信号引脚 3，对应内部中断5
const int WHEEL_PWM0 = 9;   // 1号电机转速控制引脚 9
const int WHEEL_DIR1 = 22;  // 2号电机方向控制引脚 22
const int WHEEL_FG1 = 2;   // 2号电机霍尔信号引脚 A8，对应内部中断4
const int WHEEL_PWM1 = 5;   // 2号电机转速控制引脚 5
const int WHEEL_DIR2 = 40;  // 3号电机方向控制引脚 40
const int WHEEL_FG2 = 18;   // 3号电机霍尔信号引脚 18，对应内部中断3
const int WHEEL_PWM2 = 44;  // 3号电机转速控制引脚 44
const int WHEEL_DIR3 = 42;  // 4号电机方向控制引脚 42
const int WHEEL_FG3 = 19;   // 4号电机霍尔信号引脚 19，内部中断2
const int WHEEL_PWM3 = 45;  // 4号电机方向控制引脚 45

const int MAG_IN0 = 23;    // A磁矩控制引脚 23
const int MAG_IN1 = 24;    // A磁矩控制引脚 24
const int MAG_PWMA = 6;     // A磁矩使能引脚 7
const int MAG_IN2 = 25;    // B磁矩控制引脚 A12
const int MAG_IN3 = 26;    // B磁矩控制引脚 A13
const int MAG_PWMB = 7;     // B磁矩使能引脚 2
const int MAG_IN4 = 27;    // C磁矩控制引脚 A14
const int MAG_IN5 = 28;    // C磁矩控制引脚 A15
const int MAG_PWMC = 8;     // C磁矩控制引脚 3

IMUGY85 imu;      // IMU（九轴）传感器

// 串口数据缓冲区
// 单次收发数据帧在？字节以内
uint8_t send_buffer[64];
uint8_t receiver_buffer[64];

// 电池电量记录
volatile uint8_t power_level = 9;
int volt;

// ISR（中断服务程序）中所涉及的变量需要被声明为volatile易变变量
// 状态标志量，飞轮控制，磁力矩器
volatile bool wheel_on = false;
volatile bool magnet_on = false;

// 飞轮转速记录
volatile int16_t speed_wheel1_measurement = 0;  // 飞轮1的转速测量值，-32768~32767
volatile int8_t speed_wheel1_setting = 0;      // 飞轮1的转速设定值，
volatile bool wheel1_forward = true;             // 飞轮1正转，规定顺为正逆为负
uint16_t wheel1_counter = 0;                    // 飞轮1编码计数器
volatile int16_t speed_wheel2_measurement = 0;  // 飞轮2的转速测量值，-32768~32767
volatile int8_t speed_wheel2_setting = 0;      // 飞轮2的转速设定值，
volatile bool wheel2_forward = true;             // 飞轮2正转
uint16_t wheel2_counter = 0;                    // 飞轮2编码计数器
volatile int16_t speed_wheel3_measurement = 0;  // 飞轮3的转速测量值，-32768~32767
volatile int8_t speed_wheel3_setting = 0;      // 飞轮3的转速设定值，
volatile bool wheel3_forward = true;             // 飞轮3正转
uint16_t wheel3_counter = 0;                    // 飞轮3编码计数器
volatile int16_t speed_wheel4_measurement = 0;  // 飞轮4的转速测量值，-32768~32767
volatile int8_t speed_wheel4_setting = 0;      // 飞轮4的转速设定值，
volatile bool wheel4_forward = true;             // 飞轮4正转
uint16_t wheel4_counter = 0;                    // 飞轮4编码计数器

// 磁矩设置值
volatile uint8_t x_magnetmoment_setting = 0;
volatile uint8_t y_magnetmoment_setting = 0;
volatile uint8_t z_magnetmoment_setting = 0;
volatile bool x_magnet_forward = true;    // X轴磁矩正向
volatile bool y_magnet_forward = true;    // Y轴磁矩正向      
volatile bool z_magnet_forward = true;    // Z轴磁矩正向

// IMU数据
float eulers[3] = {0};
float angular_velocities[3] = {0};
float accelerations[3] = {0};

// 速度转换函数
int16_t measurement(uint16_t wheel_counter, bool wheel_forward){
  int16_t speed_wheel_measurement = wheel_counter/9/0.5*60;
  if(!wheel_forward){
    speed_wheel_measurement = -speed_wheel_measurement;
  }
#ifdef SERIAL_DEBUG
  Serial.print("当前转速：");
  Serial.print(speed_wheel_measurement);
  Serial.println("r/min");
#endif
  return speed_wheel_measurement;
}

// 中断函数
void counter1(){wheel1_counter++;}
void counter2(){wheel2_counter++;}
void counter3(){wheel3_counter++;}
void counter4(){wheel4_counter++;}

// 计时器函数
void pertime(){
  speed_wheel1_measurement = measurement(wheel1_counter, wheel1_forward);
  speed_wheel2_measurement = measurement(wheel2_counter, wheel2_forward);
  speed_wheel3_measurement = measurement(wheel3_counter, wheel3_forward);
  speed_wheel4_measurement = measurement(wheel4_counter, wheel4_forward);
  wheel1_counter = 0;
  wheel2_counter = 0;
  wheel3_counter = 0;
  wheel4_counter = 0;
} 

// 通过串口接收上位机指令
defineTask(ReceiverThread);

void ReceiverThread::setup(){
}

void ReceiverThread::loop(){
  // 关闭所有模块设置
  if(receiver_buffer[1] = 0x00){
    wheel_on = false;
    magnet_on = false;
  }

  // 飞轮转速设定
  else if(receiver_buffer[] = 0x){
    if(receiver_buffer[] > 0){
      wheel1_forward = true;
      speed_wheel1_setting = receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR0, LOW);
    }
    else{
      wheel1_forward = false;
      speed_wheel1_setting = -receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR0, HIGH);
    }
    if(receiver_buffer[] > 0){
      wheel2_forward = true;
      speed_wheel2_setting = receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR1, LOW);
    }
    else{
      wheel2_forward = false;
      speed_wheel2_setting = -receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR1, HIGH);
    }
    if(receiver_buffer[] > 0){
      wheel3_forward = true;
      speed_wheel3_setting = receiver_buffer[]*255/100;
      digitalWrite(WHEEL_FG2, LOW);
    }
    else{
      wheel3_forward = false;
      speed_wheel3_setting = -receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR2, HIGH);
    }
    if(receiver_buffer[] > 0){
      wheel4_forward = true;
      speed_wheel4_setting = receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR3, LOW);
    }
    else{
      wheel4_forward = false;
      speed_wheel4_setting = -receiver_buffer[]*255/100;
      digitalWrite(WHEEL_DIR3, HIGH);
    }
    #ifdef SERIAL_DEBUG
        Serial.print("设定飞轮转速为：");
    #endif
  }
  // 磁矩大小设定
  elseif(receiver_buffer[] == 0){
    if(receiver_buffer[] >= 0){
      x_magnet_forward = true;
      x_magnetmoment_setting = receiver_buffer[]*255/100;
      digitalWrite(MAG_IN0, HIGH);
      digitalWrite(MAG_IN1, LOW);
    }
    else{
      x_magnet_forward = false;
      x_magnetmoment_setting = -receiver_buffer[]*255/100;
      digitalWrite(MAG_IN0, LOW);
      digitalWrite(MAG_IN1, HIGH);
    }
    if(receiver_buffer[] >= 0 ){
      y_magnet_forward = true;
      y_magnetmoment_setting = receiver_buffer[]*255/100;
      digitalWrite(MAG_IN2, HIGH);
      digitalWrite(MAG_IN3, LOW);
    }
    else{
      y_magnet_forward = false;
      y_magnetmoment_setting = -receiver_buffer[]*255/100;
      digitalWrite(MAG_IN2, LOW);
      digitalWrite(MAG_IN3, HIGH);
    }
    if(receiver_buffer[] >= 0){
      z_magnet_forward = true;
      z_magnetmoment_setting = receiver_buffer[]*255/100;
      digitalWrite(MAG_IN4, HIGH);
      digitalWrite(MAG_IN5, LOW);
    }
    else{
      z_magnet_forward = false;
      z_magnetmoment_setting = -receiver_buffer[]*255/100;
      digitalWrite(MAG_IN4, LOW);
      digitalWrite(MAG_IN5, HIGH);
    }
  #ifdef DERIAL_DEBUG
      Serial.prin("设定磁矩为：");
  #endif
  }
  sleep(20);
}
void setup() {
  // put your setup code here, to run once:
  imu.init();
  Serial.begin(9600);
  delay(20);
  mySCoop.start();
  // 飞轮
  pinMode(WHEEL_DIR0, OUTPUT);
  pinMode(WHEEL_DIR1, OUTPUT);
  pinMode(WHEEL_DIR2, OUTPUT);
  pinMode(WHEEL_DIR3, OUTPUT);
  pinMode(WHEEL_FG0, INPUT);
  pinMode(WHEEL_FG1, INPUT);
  pinMode(WHEEL_FG2, INPUT);
  pinMode(WHEEL_FG3, INPUT);
  pinMode(WHEEL_PWM0, OUTPUT);
  pinMode(WHEEL_PWM1, OUTPUT);
  pinMode(WHEEL_PWM2, OUTPUT);
  pinMode(WHEEL_PWM3, OUTPUT);

  // 磁力矩器
  pinMode(MAG_IN0, OUTPUT);
  pinMode(MAG_IN1, OUTPUT);
  pinMode(MAG_IN2, OUTPUT);
  pinMode(MAG_IN3, OUTPUT);
  pinMode(MAG_IN4, OUTPUT);
  pinMode(MAG_IN5, OUTPUT);
  pinMode(MAG_PWMA, OUTPUT);
  pinMode(MAG_PWMB, OUTPUT);
  pinMode(MAG_PWMC, OUTPUT);

  // 外部中断，上升沿触发
  attachInterrupt(5, counter1, RISING);
  attachInterrupt(4, counter2, RISING);
  attachInterrupt(3, counter3, RISING);
  attachInterrupt(2, counter4, RISING);
  // 定时器中断
  MsTimer2::set(500, pertime);    // 定时器定时500ms
  MsTimer2::start();              // 开始计时
}

void loop() {
  // put your main code here, to run repeatedly:
  // IMU部分
  imu.update();
  double temp1,temp2, temp3;
  imu.getEuler(&temp1, &temp2, &temp3);
  eulers[0] = temp1;
  eulers[1] = temp2;
  eulers[2] = temp3;
  imu.getGyro(&temp1, &temp2, &temp3);
  angular_velocities[0] = temp1;
  angular_velocities[1] = temp2;
  angular_velocities[2] = temp3;
  imu.getAcceleration(&temp1, &temp2, &temp3);
  accelerations[0] = temp1;
  accelerations[1] = temp2;
  accelerations[2] = temp3;

  // 姿态调节
  if(wheel_on){
    analogWrite(WHEEL_PWM0, speed_wheel1_setting);
    analogWrite(WHEEL_PWM1, speed_wheel2_setting);
    analogWrite(WHEEL_PWM2, speed_wheel3_setting);
    analogWrite(WHEEL_PWM3, speed_wheel4_setting);
  }
  else{
    analogWrite(WHEEL_PWM0, 0);
    analogWrite(WHEEL_PWM1, 0);
    analogWrite(WHEEL_PWM2, 0);
    analogWrite(WHEEL_PWM3, 0);
  }
  // 磁棒调节
  if(magnet_on){
    analogWrite(MAG_PWMA, x_magnetmoment_setting);
    analogWrite(MAG_PWMB, y_magnetmoment_setting);
    analogWrite(MAG_PWMC, z_magnetmoment_setting);
  }
  else{
    analogWrite(MAG_PWMA, 0);
    analogWrite(MAG_PWMB, 0);
    analogWrite(MAG_PWMC, 0);
  }
   // 电池电量读取
  volt = analogRead(A9);  // 读取电量引脚的模拟值，原理图为A9脚
  power_level = map(volt, 0, 1023, 0, 10);    // 将读的数据映射到0-10的int
  Serial.print("此时电池电量：");
  Serial.println(power_level);

  // 发送数据到上位机

  Serial.write(send_buffer, 12);
  sleep(25);
  
  yield();    // 让给另一个线程



  
}
